/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file           : main.c
 * @brief          : Main program body
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2025 STMicroelectronics.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "dma.h"
#include "gpio.h"
#include "iwdg.h"
#include "usart.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "serial.h"
#include "sys.h"
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
#define SAMPLE_NUMBER 50
typedef struct
{
    float array[ADC_CHANNEL_NUM][SAMPLE_NUMBER];
} sample_buffer_t;

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define GET_CHANNEL(x) ((x) >> 7)
#define GET_DATA(x) ((x)&0x7F)
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
const static detector_flash_t default_flash = {
    .init = DEFAULT_INITIAL_VALUE,
    .address = DEFAULT_ADDRESS,
    .config =
        {
            .param =
                {
                    .k = 45.0f,
                    .b = -50.0f,
                },
        },
};

rf_detector_t rf_detector = {0};
detector_flash_t rf_detector_flash = {0};
serial_handle_t rs485_handle = NULL;
sample_buffer_t *sample_handle = NULL;
volatile adc_raw_t adc_raw = {0};
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
void Serial_RecvCallback(serial_handle_t handle);
void Serial_SendCallback(serial_handle_t handle, uint8_t *data, uint16_t size);
void Detector_Update(void);
void Flash_Init(detector_flash_t *flash, const void *default_flash);
uint8_t Flash_SaveParams(void);
void Flash_LoadParams(void);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_ADC1_Init();
  MX_USART1_UART_Init();
  MX_IWDG_Init();
  /* USER CODE BEGIN 2 */
    rs485_handle = Serial_Init(&huart1, Serial_RecvCallback, Serial_SendCallback);
    Log_Init(rs485_handle, kLogInfo);
    HAL_UARTEx_ReceiveToIdle_DMA(rs485_handle->arg, rs485_handle->payload, SERIAL_PAYLOAD_SIZE);

    HAL_ADCEx_Calibration_Start(&hadc1);
    HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc_raw.data, ADC_CHANNEL_NUM);
    sample_handle = (sample_buffer_t *)malloc(sizeof(sample_buffer_t));
    while (sample_handle == NULL)
        continue;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
    Flash_Init(&rf_detector_flash, &default_flash);
    memcpy(&rf_detector.config, &rf_detector_flash.config, sizeof(detector_config_t));
    rf_detector.header.frame = RF_DETECTOR_FRAME_HEADER;
    rf_detector.header.address = rf_detector_flash.address;
    LOGI("System init compelet.");
    while (1)
    {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
        Serial_Run(rs485_handle);
        Detector_Update();
    }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  __HAL_FLASH_SET_LATENCY(FLASH_LATENCY_1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_LSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSIDiv = RCC_HSI_DIV1;
  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

void Serial_RecvCallback(serial_handle_t handle)
{
    if (handle->payload[0] == RF_DETECTOR_FRAME_HEADER && handle->payload[1] == DEFAULT_ADDRESS)
    {
        if (!ADD8_Check(handle->payload, handle->size))
        {
            if (BIT_READ(handle->payload[2], 7))
            {
                BIT_CLEAR(handle->payload[2], 7);
                switch (handle->payload[2])
                {
                case 0x00: // 查询报文
                    rf_detector.bias_enable = handle->payload[3];
                    ADD8_Calculation((uint8_t *)&rf_detector, sizeof(rf_detector) - 1);
                    memcpy(handle->buffer, &rf_detector, sizeof(rf_detector));
                    Serial_Print(handle, handle->buffer, sizeof(rf_detector));
                    break;
                case 0x01: // 保存参数
                    if (handle->payload[3])
                    {
                        Flash_SaveParams();
                    }
                    else
                    {
                        Flash_LoadParams();
                    }
                    break;
                case 0x02: // 开始校准
                    CalibrationStartFit(&rf_detector.config.param, handle->payload[3]);
                    break;
                case 0x03: // 设置校准点
                    CalibrationPointRecord(GET_DATA(handle->payload[3]),
                                           rf_detector.voltage[GET_CHANNEL(handle->payload[3])],
                                           *(float *)&handle->payload[4]);
                    break;
                case 0x04: // 设置偏置
                    rf_detector.config.bias[GET_CHANNEL(handle->payload[3])] = *(float *)&handle->payload[4];
                    break;
                case 0x05: // 设置斜率和截距
                    rf_detector.config.param.k = *(float *)&handle->payload[4];
                    rf_detector.config.param.b = *(float *)&handle->payload[8];
                    break;
                default:
                    break;
                }
            }
            else
            {
                switch (handle->payload[2])
                {
                case 0x00: // 查询报文
                    rf_detector.bias_enable = handle->payload[3];
                    LOGI("Compilation date %s\n"
                         "RF-Detector addr: %1d, Bias: %s\n"
                         "Bias   : %6.2f | %6.2f\n"
                         "Voltage: %6.2f | %6.2f\n"
                         "dBm    : %6.2f | %6.2f\n"
                         "Watt   : %6.2f | %6.2f",
                         __DATE__, rf_detector.header.address, rf_detector.bias_enable ? "ON" : "OFF",
                         rf_detector.config.bias[0], rf_detector.config.bias[1], rf_detector.voltage[0],
                         rf_detector.voltage[1], rf_detector.dBm[0], rf_detector.dBm[1], rf_detector.power[0],
                         rf_detector.power[1]);
                    break;
                case 0x01:                       // 操作Flash
                    if (handle->payload[3] != 0) // 保存参数
                    {
                        if (Flash_SaveParams())
                            LOGI("Save success");
                    }
                    else
                    {
                        Flash_LoadParams(); // 加载参数
                        LOGI("Flash loaded, slope: %5.1f, intercept: %5.1f", rf_detector.config.param.k,
                             rf_detector.config.param.b);
                    }
                    break;
                case 0x02: // 执行校准拟合
                    if (CalibrationStartFit(&rf_detector.config.param, GET_DATA(handle->payload[3])) == 0)
                    {
                        LOGI("Calibration: slope: %5.1f, intercept: %5.1f", rf_detector.config.param.k,
                             rf_detector.config.param.b);
                    }
                    break;
                case 0x03: // 记录校准点
                    if (CalibrationPointRecord(GET_DATA(handle->payload[3]),
                                               rf_detector.voltage[GET_CHANNEL(handle->payload[3])],
                                               DataInversion(&handle->payload[4]).decimal) == 0)
                    {
                        LOGI("Recorded Channel%d Point [%d]: (%5.1f V, %5.1f dBm)",
                             GET_CHANNEL(handle->payload[3]), GET_DATA(handle->payload[3]),
                             rf_detector.voltage[GET_CHANNEL(handle->payload[3])],
                             DataInversion(&handle->payload[4]).decimal);
                    }
                    break;
                case 0x04: // 设置偏置
                    rf_detector.config.bias[GET_CHANNEL(handle->payload[3])] =
                        DataInversion(&handle->payload[4]).decimal;
                    LOGI("Set bias: Ch[%2d]: %5.1f dB", GET_CHANNEL(handle->payload[3]),
                         rf_detector.config.bias[GET_CHANNEL(handle->payload[3])]);
                    break;
                case 0x05: // 设置斜率和截距
                    rf_detector.config.param.k = DataInversion(&handle->payload[4]).decimal;
                    rf_detector.config.param.b = DataInversion(&handle->payload[8]).decimal;
                    LOGI("Set slope: %5.1f, intercept: %5.1f", rf_detector.config.param.k, rf_detector.config.param.b);
                    break;
                case 0xFF: // 系统复位
                    LOGI("System resetting...");
                    delay_ms(100);
                    System_Reset();
                    break;
                default:
                    break;
                }
            }
        }
    }
    HAL_UARTEx_ReceiveToIdle_DMA(handle->arg, handle->payload, SERIAL_PAYLOAD_SIZE);
}

void Serial_SendCallback(serial_handle_t handle, uint8_t *data, uint16_t size)
{
    HAL_UART_Transmit_DMA((UART_HandleTypeDef *)handle->arg, data, size);
}

void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
{
    if (huart == (UART_HandleTypeDef *)rs485_handle->arg)
    {
        Serial_RecvComplete(rs485_handle, Size);
    }
}

void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
{
    if (huart == (UART_HandleTypeDef *)rs485_handle->arg)
    {
        Serial_SendComplete(rs485_handle);
    }
}

void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart)
{
    if (huart == (UART_HandleTypeDef *)rs485_handle->arg)
    {
        HAL_UARTEx_ReceiveToIdle_DMA(huart, rs485_handle->payload, SERIAL_PAYLOAD_SIZE);
    }
}

/* 1D 卡尔曼滤波器（通道化）：
 * channel: 通道索引
 * measurement: 当前测量值（电压）
 * 返回滤波后电压
 */
static float Kalman1D_Update(uint8_t channel, float measurement)
{
    static uint8_t inited[ADC_CHANNEL_NUM] = {0};
    static float x[ADC_CHANNEL_NUM]; // 状态估计（电压
    static float p[ADC_CHANNEL_NUM]; // 估计协方差
    static float q[ADC_CHANNEL_NUM]; // 过程噪声
    static float r[ADC_CHANNEL_NUM]; // 测量噪声

    if (channel >= ADC_CHANNEL_NUM)
        return measurement;

    if (!inited[channel])
    {
        x[channel] = measurement;
        p[channel] = 1.0f;  // 初始协方差
        q[channel] = 1e-3f; // 过程噪声(可调)
        r[channel] = 0.01f;  // 测量噪声(可调)
        inited[channel] = 1;
    }

    // 预测
    float x_pred = x[channel];
    float p_pred = p[channel] + q[channel];

    // 更新
    float K = p_pred / (p_pred + r[channel]);
    x[channel] = x_pred + K * (measurement - x_pred);
    p[channel] = (1.0f - K) * p_pred;

    return x[channel];
}

void Detector_Update(void)
{
#define RUN_INTERVAL_MS 500
#define SAMPLING_INTERVAL_MS 50

    static uint8_t index = 0;

    static uint32_t last_run_time = 0;
    static uint32_t last_sample_time = 0;

    uint32_t tick = HAL_GetTick();
    float sum_voltages[ADC_CHANNEL_NUM] = {0};

    // 采样
    if ((int32_t)(tick - last_sample_time) >= 0)
    {
        last_sample_time = tick + SAMPLING_INTERVAL_MS;

        uint16_t adc_snapshot[ADC_CHANNEL_NUM];
        memcpy(adc_snapshot, (const uint16_t *)adc_raw.data, sizeof(adc_snapshot));

        for (uint8_t i = 0; i < ADC_CHANNEL_NUM; i++)
        {
            // 卡尔曼滤波
            float meas_voltage = ((float)adc_snapshot[i]) * (DEFAULT_VREF_VOLTAGE / DEFAULT_ADC_RESOLUTION);
            sample_handle->array[i][index] = Kalman1D_Update(i, meas_voltage);
            // 移动平均
            for (uint8_t j = 0; j < SAMPLE_NUMBER; j++)
            {
                sum_voltages[i] += sample_handle->array[i][j];
            }
            rf_detector.voltage[i] = sum_voltages[i] / SAMPLE_NUMBER;
            rf_detector.dBm[i] =
            CalculateVoltageToDBm(rf_detector.voltage[i], rf_detector.config.param.k, rf_detector.config.param.b) +
            (rf_detector.bias_enable ? rf_detector.config.bias[i] : 0.0f);
            rf_detector.power[i] = CalculateDBmToWatt(rf_detector.dBm[i]);
        }
        index = (index + 1) % SAMPLE_NUMBER; // 循环计数

        // 数据可视化
        // static uint8_t test_data[20] = {0};
        // memcpy(test_data, &rf_detector.voltage[0], sizeof(float));
        // memcpy(test_data + 4, &rf_detector.dBm[0], sizeof(float));
        // memcpy(test_data + 8, &rf_detector.voltage[1], sizeof(float));
        // memcpy(test_data + 12, &rf_detector.dBm[1], sizeof(float));
        // test_data[18] = 0x80;
        // test_data[19] = 0x7f;
        // Serial_Print(rs485_handle, test_data, sizeof(test_data));
    }

    // 周期性更新
    if ((int32_t)(tick - last_run_time) >= 0)
    {
        last_run_time = tick + RUN_INTERVAL_MS;
        LED_TOGGLE();
        __HAL_IWDG_RELOAD_COUNTER(&hiwdg);
    }
}

void Flash_Init(detector_flash_t *flash, const void *default_flash)
{
#if !ENABLE_DEBUG_MODE
    detector_flash_t temp;

    flash->init = DEFAULT_INITIAL_VALUE;
    Flash_Read(FLASH_START_ADDR, (flash_data_t *)&temp, sizeof(detector_flash_t) / sizeof(flash_data_t));
    if (temp.init != flash->init)
    {
        memcpy(flash, default_flash, sizeof(detector_flash_t));
        LOGI("Flash is initialized");
        if (Flash_Write(FLASH_START_ADDR, (flash_data_t *)flash, sizeof(detector_flash_t) / sizeof(flash_data_t)))
        {
            LOGE("Flash write failed");
        }
    }
    else
    {
        memcpy(flash, &temp, sizeof(detector_flash_t));
        LOGI("Read flash parameters");
    }
#else
    memcpy(flash, default_flash, sizeof(sub_control_flash_t));
#endif
}

uint8_t Flash_SaveParams(void)
{
    memcpy(&rf_detector_flash.config, &rf_detector.config, sizeof(detector_config_t));
    return Flash_Write(FLASH_START_ADDR, (flash_data_t *)&rf_detector_flash,
                       sizeof(detector_flash_t) / sizeof(flash_data_t));
}

void Flash_LoadParams(void)
{
    Flash_Read(FLASH_START_ADDR, (flash_data_t *)&rf_detector_flash, sizeof(detector_flash_t) / sizeof(flash_data_t));
    memcpy(&rf_detector.config, &rf_detector_flash.config, sizeof(detector_config_t));
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
    /* User can add his own implementation to report the HAL error return state */
    __disable_irq();
    while (1)
    {
    }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
    /* User can add his own implementation to report the file name and line number,
       ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
